
/*
  Quadruped Robot: trot gait
*/
#ifndef GAIT_SERVO_H
#define GAIT_SERVO_H

#include <ESP32Servo.h>

// 0,12,13无效，不能使用GPIO18,19，会使芯片的USB失效
//  Define servo pins
#define front_left_PIN 1  // front-left leg
#define rear_left_PIN  6   // back-left leg

#define front_right_PIN 3  // front-right leg
#define rear_right_PIN 10   // back-right leg

// Define 4 servos (one for each leg)
Servo front_left;
Servo front_right;
Servo rear_left;
Servo rear_right;

// gait action code
const int GAIT_TROT_FORWARD = 1;   // F
const int GAIT_TROT_BACKWARD = 2;  // B
const int GAIT_TROT_LEFT = 3;      // L
const int GAIT_TROT_RIGHT = 4;     // R
const int GAIT_STANDING_UP = 5;    // U 站
const int GAIT_SQUAT = 6;          // Q 蹲
const int GAIT_SIT = 7;            // S 坐下
const int GAIT_DOWN = 8;           // D 躺下

// gait state
int gait_state = GAIT_STANDING_UP;

int step_speed = 300;  // speed- delay between steps (ms)
// Calibration positions (adjust according to your mounting)
// 定义舵机最小和最大角度
const int min_angle = 0;
const int max_angle = 180;
int neutral_pos = 90;
int step_height = 50;
int lift_left_angle = neutral_pos - step_height;  // Leg lifted position
int push_left_angle = neutral_pos + step_height;  // Leg pushing position
int lift_right_angle = neutral_pos + step_height;
int push_right_angle = neutral_pos - step_height;

int testDelay = 1000;  // Delay between steps (ms)

// test legs
void test_leg(Servo leg, int lift_angle, int push_angle) {
  leg.write(neutral_pos);
  delay(testDelay);
  leg.write(lift_angle);
  delay(testDelay);
  leg.write(neutral_pos);
  delay(testDelay);
  leg.write(push_angle);
  delay(testDelay);
  leg.write(neutral_pos);
  delay(testDelay);
}

// move body forward
void move_body_forward() {
  front_left.write(neutral_pos);
  front_right.write(neutral_pos);
  rear_left.write(neutral_pos);
  rear_right.write(neutral_pos);
}

// Reset all servos to neutral position
void reset_position() {
  move_body_forward();
  delay(step_speed);
}

void standing_up_position() {
  reset_position();
}

void squatting_position() {
  front_left.write(lift_left_angle);
  rear_left.write(push_left_angle);
  front_right.write(lift_right_angle);
  rear_right.write(push_right_angle);
  delay(2 * step_speed);

  move_body_forward();
  delay(2 * step_speed);

  front_left.write(push_left_angle);
  rear_left.write(lift_left_angle);
  front_right.write(push_right_angle);
  rear_right.write(lift_right_angle);
  delay(2 * step_speed);
}

void sit_position() {
  front_left.write(neutral_pos);
  front_right.write(neutral_pos);
  rear_left.write(max_angle);
  rear_right.write(min_angle);
  delay(step_speed);
}

void down_position() {
  front_left.write(min_angle);
  front_right.write(max_angle);
  rear_left.write(max_angle);
  rear_right.write(min_angle);
  delay(step_speed);
}

/*-----------------------------------------------------------
  trot gait: diagonal legs as pair
--------------------------------------------------------------*/
// trot walk forward
void trot_walk_forward() {
  // Step 1.1: lift front-left and rear-right legs
  front_left.write(lift_left_angle);
  rear_right.write(lift_right_angle);
  // Step 1.2: push front-right and rear-left legs
  front_right.write(push_right_angle);
  rear_left.write(push_left_angle);
  delay(step_speed);
  // Step 2: move body forward
  move_body_forward();
  delay(step_speed);
  // Step 3.1: lift front-right and rear-left legs
  front_right.write(lift_right_angle);
  rear_left.write(lift_left_angle);
  // Step 3.2: push front-left and rear-right legs
  front_left.write(push_left_angle);
  rear_right.write(push_right_angle);
  delay(step_speed);
  // Step 4: move body forward
  move_body_forward();
  delay(step_speed);
}

// trot walk backward
void trot_walk_backward() {
  // Step 1.1: push rear-right and fornt-left backward
  rear_right.write(push_right_angle);
  front_left.write(push_left_angle);
  // Step 1.2: lift rear-left and front-right legs backward
  rear_left.write(lift_left_angle);
  front_right.write(lift_right_angle);
  delay(step_speed);
  // Step 2: move body forward
  move_body_forward();
  delay(step_speed);
}

// trot turn left(trot gait)
void trot_turn_left() {
  // Step 1.1: lift front_right leg and push rear_left
  front_right.write(lift_right_angle);
  rear_left.write(push_left_angle);
  //delay(step_speed);
  // Step 1.2:  lift rear_right leg and push front_left
  rear_right.write(lift_right_angle);
  front_left.write(push_left_angle);
  delay(step_speed);
  // Step 2: move body forward
  move_body_forward();
  delay(step_speed);
}

// trot turn right(trot gait)
void trot_turn_right() {
  // Step 1.1: lift front_left leg and push rear_right
  front_left.write(lift_left_angle);
  rear_right.write(push_right_angle);
  // Step 1.2:  lift rear_left leg and push front_right
  rear_left.write(lift_left_angle);
  front_right.write(push_right_angle);
  delay(step_speed);
  // Step 2: move body forward
  move_body_forward();
  delay(step_speed);
}

void gait_action(int gait_cmd) {
  switch (gait_cmd) {
    case GAIT_TROT_FORWARD:
      trot_walk_forward();
      gait_state = GAIT_TROT_FORWARD;
      break;
    case GAIT_TROT_LEFT:
      trot_turn_left();
      gait_state = GAIT_TROT_LEFT;
      break;
    case GAIT_TROT_RIGHT:
      trot_turn_right();
      gait_state = GAIT_TROT_RIGHT;
      break;
    case GAIT_TROT_BACKWARD:
      trot_walk_backward();
      gait_state = GAIT_TROT_BACKWARD;
      break;
    case GAIT_STANDING_UP:
      standing_up_position();
      gait_state = GAIT_STANDING_UP;
      break;
    case GAIT_SQUAT:
      squatting_position();
      gait_state = GAIT_SQUAT;
      break;
    case GAIT_DOWN:
      down_position();
      gait_state = GAIT_DOWN;
      break;
    case GAIT_SIT:
      sit_position();
      gait_state = GAIT_SIT;
      break;
    default:
      break;
  }  // switch
};

void setup_gait() {
  // Attach servos to pins
  front_left.attach(front_left_PIN);
  front_right.attach(front_right_PIN);
  rear_left.attach(rear_left_PIN);
  rear_right.attach(rear_right_PIN);
  // Set servos to neutral position
  reset_position();
  //test left leg
  //test_leg(front_left, lift_left_angle, push_left_angle);
  //delay(2000);
  //test_leg(rear_left, lift_left_angle, push_left_angle);
  //delay(2000);

  //test right legs
  //test_leg(front_right,lift_right_angle, push_right_angle);
  //delay(2000);
  //test_leg(rear_right,lift_right_angle, push_right_angle);
  //delay(2000);

  // for (int i = 0; i <= 10; i++) { trot_walk_forward(); }
}

void loop_gait() {
  gait_action(gait_state);
}

#endif /* GAIT_SERVO_H */